/*
 * @Author: EltonLi lijiaming@uavi-tech.com
 * @Date: 2023-10-08 14:13:16
 * @LastEditors: LVGRAPE
 * @LastEditTime: 2025-04-09 17:28:45
 * @FilePath: \zino-fc-v4\applications\ZINO_FC_V4.0\optical_flow\optical_flow.c
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include <rtthread.h>
#include "zino.h"
#include <sensor.h>

#include "optical_flow.h"
#include "shanwai.h"
#include <sensor.h>
#include <math.h>
#include "imu.h"
#include "maths.h"
#include "filter.h"
#include "fc_error.h"
#define DBG_TAG "opticalFlow"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>
#include "statecal.h"

static biquadFilter_t optFilter_bf[2];
static biquadFilter_t gyroFilter_bf[2];


// /**
//  * @description: 不做数据融合，直接以光流原始数据作为速度，用这个函数的时候不要与陀螺仪做融合
//  * @param {fc_sensor_data_t} *sensor    姿态数据
//  * @param {zdrone_state_t} *state   状态数据
//  * @param {rt_sensor_data} *data    原始数据
//  * @param {float} dt                计算时间
//  * @return {*}
//  */
// void opticalFlow_Update(fc_sensor_data_t *sensor, zdrone_state_t *state, struct rt_sensor_data *data, float dt)
// {
//     if (!sensor->optf_raw.isOpFlowOk)
//         return;
//     float coeff = RESOLUTION * sensor->tof.range * 0.001f; // 单位是m，激光初始数据单位是mm
//     float tanRoll = tanf(DEGREES_TO_RADIANS(state->attitude.roll));
//     float tanPitch = tanf(DEGREES_TO_RADIANS(state->attitude.pitch));

//     sensor->optf.optfix[X] = 480.f * tanPitch; /*像素补偿，负方向*/
//     sensor->optf.optfix[Y] = 480.f * tanRoll;

//     sensor->optf.vel[X] = coeff*(data->data.of.x - sensor->optf.optfix[X]);
//     sensor->optf.vel[Y] = coeff*(data->data.of.y - sensor->optf.optfix[Y]);
//     sensor->optf.pos[X] += sensor->optf.vel[X] * dt;
//     sensor->optf.pos[Y] += sensor->optf.vel[Y] * dt;
// }

void opticalFlow_apply_layout(int8_t layout, struct rt_sensor_data *opt, fc_sensor_data_t *sensor)
{

    /**
     *
     */
    switch (layout)
    {
    /**光流摄像头朝向地面*/
    case 0: // X向前
        sensor->optf_raw.optVelX = opt->data.of.y;
        sensor->optf_raw.optVelY = -opt->data.of.x;
        break;
    case 1: // X向后
        sensor->optf_raw.optVelX = -opt->data.of.y;
        sensor->optf_raw.optVelY = opt->data.of.x;
        break;
    case 2: // Y向前
        sensor->optf_raw.optVelX = -opt->data.of.x;
        sensor->optf_raw.optVelY = -opt->data.of.y;
        break;
    case 3: // Y向后
        sensor->optf_raw.optVelX = opt->data.of.x;
        sensor->optf_raw.optVelY = -opt->data.of.y;
        break;
    default:
        break;
    }
}

void opticalFlowDataUpdate(fc_sensor_data_t *sensor, zdrone_state_t *state)
{
    sensor->optf_raw.lpfX = biquadFilterApply(&optFilter_bf[X], sensor->optf_raw.optVelX);
    sensor->optf_raw.lpfY = biquadFilterApply(&optFilter_bf[Y], sensor->optf_raw.optVelY);

    float gxLpf = biquadFilterApply(&gyroFilter_bf[X], state->attitudeRate.roll * 0.1f);
    float gyLpf = biquadFilterApply(&gyroFilter_bf[Y], state->attitudeRate.pitch * 0.1f);

    // float absX = fabsf(sensor->optf_raw.lpfX);
    // float absY = fabsf(sensor->optf_raw.lpfY);

    float fixX = sensor->optf_raw.lpfX - gxLpf;
    float fixY = sensor->optf_raw.lpfY - gyLpf;

    // float fixX = sensor->optf_raw.optVelX - state->attitudeRate.roll * 0.1f;
    // float fixY = sensor->optf_raw.optVelY - state->attitudeRate.pitch * 0.1f;

    // sensor->optf_raw.fixVelX += (fixX - sensor->optf_raw.fixVelX) * 0.81f;
    // sensor->optf_raw.fixVelY += (fixY - sensor->optf_raw.fixVelY) * 0.81f;

    sensor->optf_raw.fixVelX = fixX;
    sensor->optf_raw.fixVelY = fixY;

    // sensor->optf_raw.fixVelX = sensor->tof.range / 10.0f * 0.01f * 7.754f * sensor->optf_raw.fixVelX;
    // sensor->optf_raw.fixVelY = sensor->tof.range / 10.0f * 0.01f * 7.754f * sensor->optf_raw.fixVelY;

    // sensor->optf_raw.fixPosX += sensor->optf_raw.fixVelX * 0.01f;
    // sensor->optf_raw.fixPosY += sensor->optf_raw.fixVelY * 0.01f;
}

void opticalFlowTask(void *parameter)
{
#define QUICK_MEDIAN_FILTER_SIZE 5
    static uint16_t count = 0;
    rt_device_t pmw_opt = RT_NULL;
    struct rt_sensor_data opt;
    // static int currentFilterSampleIndex = 0;
    // static bool medianFilterReady = false;
    // static float optX_trace[QUICK_MEDIAN_FILTER_SIZE];
    // static float optY_trace[QUICK_MEDIAN_FILTER_SIZE];

    pmw_opt = rt_device_find("of_pmw39");
    if (pmw_opt)
        rt_device_open(pmw_opt, RT_DEVICE_FLAG_RDWR);
    else
    {
        rt_kprintf("can't find optical flow sensor\n");
        FC_ERROR_SET(FC_OPTICALFLOW_HARDFAULT);
        return;
    }

    biquadFilterInitLPF(&optFilter_bf[X], 50, 1000); // NOTE LPF截止频率只能是刷新率的一半
    biquadFilterInitLPF(&optFilter_bf[Y], 50, 1000); // NOTE LPF截止频率只能是刷新率的一半

    biquadFilterInitLPF(&gyroFilter_bf[X], 40, 1000);
    biquadFilterInitLPF(&gyroFilter_bf[Y], 40, 1000);

    while (1)
    {
        rt_device_read(pmw_opt, 0, &opt, 1);
        if (opt.data.of.x == 0 && opt.data.of.y == 0)
        {
            if (count++ > 100 && fc_sensor_data.optf_raw.isOpFlowOk)
            {
                count = 0;
                fc_sensor_data.optf_raw.isOpFlowOk = false;
            }
        }
        else
        {
            fc_sensor_data.optf_raw.isOpFlowOk = true;
            count = 0;
        }
        opticalFlow_apply_layout(3, &opt, &fc_sensor_data);
        opticalFlowDataUpdate(&fc_sensor_data, &zdrone_state);
        fc_sensor_data.optf_raw.optQual = opt.data.of.quality;
        fc_sensor_data.optf_raw.shutter = opt.data.of.shutter;
        fc_sensor_data.optf_raw.is_valid = opt.data.of.is_valid;
#if 0
        static uint8_t debugCnt = 0;
        if (debugCnt++ >= 2)
        {
            debugCnt = 0;
            digitalOsc_Head();
            digitalOsc_printf("f1", (float)pixelDx);
            digitalOsc_printf("f1", (float)pixelDy);
            digitalOsc_printf("f1", (float)fc_sensor_data.optf_raw.lpfX);
            digitalOsc_printf("f1", (float)fc_sensor_data.optf_raw.lpfY);
            digitalOsc_printf("f1", (float)gxLpf);
            digitalOsc_printf("f1", (float)gyLpf);
            digitalOsc_printf("f1", (float)fc_sensor_data.optf_raw.fixX);
            digitalOsc_printf("f1", (float)fc_sensor_data.optf_raw.fixY);
            //  digitalOsc_printf("f1", fc_sensor_data.optf_raw.optPosY);
            digitalOsc_End();
        }
#endif
        rt_thread_mdelay(10);
    }
}

int opticalFlowTaskEntry(void)
{
    rt_thread_t opticalFlow_thread = rt_thread_create("opticalFlow", opticalFlowTask, RT_NULL, 1024, 10, 10);
    if (opticalFlow_thread != RT_NULL)
    {
        rt_thread_startup(opticalFlow_thread);
    }
    else
    {
        LOG_E("opticalFlow_thread create failed.");
    }
    return 0;
}

// ZINO_APP_EXPORT(opticalFlowTaskEntry);
